#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  ultraSonic,     sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  rightTouch,     sensorTouch)
#pragma config(Sensor, dgtl8,  leftTouch,      sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int topSpeed = 100;
int lowSpeed = 50;




void forward1(){
  motor(rightMotor) = 110;
  motor(leftMotor) = 127;
}

void turnRight(int degs){
  SensorValue[leftEncoder]=0;
  SensorValue[rightEncoder]=0;
  while(SensorValue[leftEncoder]<degs){
    motor[leftMotor]=80;
    motor[rightMotor]=80;
  }
}

void forward(int revs){
  int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]<turns ){
    if(rightEncoder>leftEncoder){
      motor[rightMotor]=75;
      motor[leftMotor]=80;
    }
    else if(rightEncoder<leftEncoder){
      motor[rightMotor]=80;
      motor[leftMotor]=75;
    }
    else{
      motor[rightMotor]=80;
      motor[leftMotor]=80;
    }
  }
}

void backUp(){
  motor[rightMotor]= 0;
  motor[leftMotor]= 0;
  wait1Msec(200);
  SensorValue[rightEncoder]=0;
  SensorValue[rightEncoder]=0;
  while(SensorValue[rightEncoder]<-125){
    motor[rightMotor]= -100;
    motor[leftMotor]= -100;
  }
  turnRight(100);

}

void slightRight(){
  backUp();
  SensorValue[leftEncoder]=0;
  SensorValue[rightEncoder]=0;
  while(SensorValue[leftEncoder]<100){
    motor[leftMotor]=80;
    motor[rightMotor]=-80;
 }
}

void slightLeft(){
  backUp();
  SensorValue[leftEncoder]=0;
  SensorValue[rightEncoder]=0;
  while(SensorValue[leftEncoder]<4){
    motor[leftMotor]=-80;
    motor[rightMotor]=80;
 }
}

task main(){

    while(1==1){
      while(SensorValue(ultraSonic)>75 || SensorValue(ultraSonic)< 0){

          if(SensorValue[rightTouch]==0 & SensorValue[leftTouch]==0){
            forward(1);
          }
          else if(SensorValue[rightTouch]==1){
            slightLeft();
          }
          else if(SensorValue[leftTouch]==1){
            slightRight();
          }
      }
     turnRight(400)
  }
}
